import math

def calculate_total_trajectory_length(filename):
    """
    计算TUM格式文件中轨迹的总里程长度。

    :param filename: str, 输入TUM格式文件名
    :return: float, 轨迹的总里程长度
    """
    total_length = 0.0
    previous_point = None

    with open(filename, 'r') as file:
        for line in file:
            values = line.strip().split()
            x = float(values[1])
            y = float(values[2])
            z = float(values[3])

            current_point = (x, y, z)

            if previous_point is not None:
                # 计算当前点和前一个点之间的欧几里得距离
                distance = math.sqrt(
                    (current_point[0] - previous_point[0]) ** 2 +
                    (current_point[1] - previous_point[1]) ** 2 +
                    (current_point[2] - previous_point[2]) ** 2
                )
                total_length += distance

            previous_point = current_point

    return total_length

# corridor/gt_0517_corridor_00.txt
# indoor/gt_0517_indoor_00.txt
# park/gt_0705_parking_00.txt
# running/gt_0705_running_00.txt


data_dir = '../log/A1/running/'
data_file = 'gt_0705_running_00.txt'


filename = data_dir + data_file

total_length = calculate_total_trajectory_length(filename)
print(f"Total trajectory length: {total_length}")